/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/publicInterfaceSrc/follow_path.cpp
 * @Description  : 实现了路径跟随的控制算法。
 * 				   代码中定义了不同的纠偏控制器，包括PID和LQR控制器，用于根据车辆的当前姿态和目标轨迹计算所需的控制指令。
 *                 主要功能包括清除PID控制设置、根据不同条件选择合适的纠偏算法（如全向车和非全向车的处理）、处理半自动路径跟随等。
 *                 代码通过计算角度和位置偏差来调整车辆的运动路径，以确保车辆准确地沿着预定轨迹行驶。
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:24:54
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#include "follow_path.h"
#include "tools.h"
#include "public_kernel.h"
PidFollow pid_follow;		     // 自动运行时纠偏
PidFollow pid_follow_half_auto;  // 磁条巡线半自动纠偏
LqrFollow lqr_follow;

// LQRControler LQR;
/**
 * @brief 清除PID控制
 *
 * 当输入的末端速度接近0时，清除相关的PID控制设置。
 *
 * @param end_velocity 末端速度
 */
void ClearPidCtrl(float end_velocity)
{
	if (fabs(end_velocity) < 0.0001f) {
		GuideHelper::modify_ctrl_.Clear();
		pid_follow.Clear();
		DEBUG_OUT("ClearPidCtrl and global modify ctrl value once: end_velocity=" << end_velocity);
  	}
}

/**
 * @brief 清除半自动PID控制器的设置
 *
 * 当输入的结束速度接近0时，清除半自动PID控制器的设置。
 *
 * @param end_velocity 结束速度，单位为米/秒
 */
void ClearHalfAutoPidCtrl(float end_velocity) 
{
	if (fabs(end_velocity) < 0.0001f) {
		pid_follow_half_auto.Clear();
		DEBUG_OUT("ClearHalfAutoPidCtrl once: end_velocity=" << end_velocity);
	}
}

/**
 * @brief 根据当前姿态和轨迹进行路径跟随控制
 *
 * 该函数根据给定的当前姿态和轨迹，使用不同的控制算法（如PID、LQR等）来计算跟随路径所需的控制数据。
 * 纠偏方向：实际车体车头在直线的左侧，fx<0(偏左，也就是y的正方向) fx:左负右正
 * 	左右纠偏：      #1 v>0时，fx <0，w < 0; fx > 0,w > 0;(即正走时，fx和w符号相同)
 * 				   #2 v<0时，fx <0，w > 0; fx > 0,w > 0;(即倒走时，fx和w符号相反	
 * 	角度纠偏:       #3 v>0或v<0时，fthita > 0 ,w <0 ; fthita < 0 ,w >0 ;
 * 	
 * @param pst_measured 当前的姿态
 * @param traj 轨迹对象
 * @param velocity 当前速度
 * @param ky 纠偏算法中Y比例系数(对于非全向车)
 * @param ka 纠偏算法中A比例系数
 * @param ignore_y_error 是否忽略y方向误差
 *
 * @return 返回控制数据
 */
ModifyCtrl FollowPath(const Posture* pst_measured, Trajectory* traj, float velocity, float ky, float ka, bool ignore_y_error)
{
	DEBUG_OUT("FollowPath start");

	// Calculate deviations
	ProjectData deviation;
	bool estimate = traj->ProjectPosture(const_cast<Posture&>(*pst_measured), &deviation);

	// 轨迹纠偏theta角度，用来计算vx vy 分量
	Angle angle_to_tangent = traj->HeadToTangent();
	State current_point, pre_point;

#ifdef AGV_DEBUG_NAV
	deviation.theta_ = 0.01f;
	deviation.x_ = 0.02f;
	DEBUG_OUT("agv debug nav set deviation.theta_=" << deviation.theta_ << ", deviation.x_=" << deviation.x_);
#endif

	if (ignore_y_error) {
		DEBUG_OUT("ignoreYError force set deviation.x_ is 0.0; deviation.x_=" << deviation.x_);
		deviation.x_ = 0.0f;
	}

	ModifyCtrl ctrl_data;
	ctrl_data.Clear();

	// #1 followPath 纠偏算法
	if (!pid_follow.PidEnable() && !lqr_follow.LqrEnable(velocity)) {
		// followpatah纠偏算法
		float t1 = cos(deviation.theta_);
		float t2 = sin(deviation.theta_);
		float t4 = -ky * t1 * t1 * deviation.x_;
		float t5 = t2 * (ka * t1 * SIGN(velocity * t1));
		ctrl_data.angular_velocity = -velocity * t1 * (t4 + t5);

		DEBUG_OUT("follow: t1: " << t1 << ", t2: " << t2 << ", velocity: " << velocity
									<< ", deviation.x_: " << deviation.x_ << ", ky: " << ky << ", ka: " << ka
									<< ", t4: " << t4 << ", deviation.theta_: " << deviation.theta_ << ", t5: " << t5
									<< ", w: " << ctrl_data.angular_velocity << ", deltaVx: " << ctrl_data.delta_velocity_x
									<< ", deltaVy: " << ctrl_data.delta_velocity_y << ", estimate: " << estimate);

		if (fabs(velocity) <= IGNORE_CORRECT_VEL) {
			ctrl_data.Clear();
			DEBUG_OUT("follow: force ignore correct value(w); velocity=" << velocity << ", ignore_correct_vel=" << IGNORE_CORRECT_VEL);
		}
	} else if (pid_follow.PidEnable() && !lqr_follow.LqrEnable(velocity)) {
		// #2 PID纠偏算法
		bool omni_direction = IsOmniDirection(); // 是否时全向车型
		if (!omni_direction) {
			// AMR  pid纠偏算法
			float error_y = SIGN(velocity) * deviation.x_;
			if (fabs(pid_follow.WheelBase()) > 0.0001) {
				error_y = SIGN(velocity) * atan(deviation.x_ / pid_follow.WheelBase());
			}
			float error_a = -Angle::NormAngle2(deviation.theta_); //[-PI,PI]
																	// 将y方向偏差 -> 转化为角度偏差
			pid_follow.SetPidFilter(velocity);
			float a_ctrl = pid_follow.pid_a.Generate(error_a, velocity);
			float y_ctrl = pid_follow.pid_y.Generate(error_y, velocity);
			ctrl_data.angular_velocity = a_ctrl + y_ctrl;

			DEBUG_OUT("pid: amr: ctrl_data.fW=" << ctrl_data.angular_velocity << ", fVelX=" << ctrl_data.delta_velocity_x
												<< ", fVelY=" << ctrl_data.delta_velocity_y << ", error_y=" << error_y
												<< ", error_a: " << error_a << ", a_ctrl: " << a_ctrl
												<< ", y_ctrl: " << y_ctrl << ", deviation.x_: " << deviation.x_
												<< ", deviation.theta_: " << deviation.theta_ << ", velocity: " << velocity
												<< ", estimate: " << estimate);
		} else {
			// 全向车 pid纠偏算法
			float error_y = SIGN(velocity) * deviation.x_;   // Y 方向偏差
			float error_a = -Angle::NormAngle2(deviation.theta_); //[-PI,PI]
			pid_follow.SetPidFilter(velocity);
			float a_ctrl = pid_follow.pid_a.Generate(error_a, velocity);
			float y_ctrl = pid_follow.pid_y.Generate(error_y, velocity);
			ctrl_data.delta_velocity_x = sin(angle_to_tangent) * y_ctrl;
			ctrl_data.delta_velocity_y = cos(angle_to_tangent) * y_ctrl;
			ctrl_data.angular_velocity = a_ctrl;

			DEBUG_OUT("pid: omni: ctrl_data.fW=" << ctrl_data.angular_velocity << ", fVelX=" << ctrl_data.delta_velocity_x
												<< ", fVelY=" << ctrl_data.delta_velocity_y << ", error_y=" << error_y
												<< ", error_a=" << error_a << ", angle_to_tangent(degree)="
												<< angle_to_tangent.Degree() << ", a_ctrl=" << a_ctrl
												<< ", y_ctrl=" << y_ctrl << ", velocity=" << velocity
												<< ", estimate=" << estimate);
		}
	} else if (lqr_follow.LqrEnable(velocity)) {
		DEBUG_OUT("[LQRFollow] start calculating");
		lqr_follow.read_lqr_param();
		current_point.pos.x = 0.0;
		current_point.pos.y = -deviation.x_;
		current_point.pos.yaw = Angle::NormAngle2(deviation.theta_);
		current_point.speed.v = velocity;
		current_point.speed.w = 0.0;
		pre_point.pos.x = lqr_follow.lqr.pre_point_dist_;
		pre_point.pos.y = 0.0;
		pre_point.pos.yaw = 0.0;
		pre_point.speed.v = velocity;
		Speed out = lqr_follow.lqr.LQRControl(current_point, pre_point);
		ctrl_data.angular_velocity = out.w;

		DEBUG_OUT("[LQRFollow] current_point: x, y, theta: " << current_point.pos.x << ", "
																	<< current_point.pos.y << ", "
																	<< current_point.pos.yaw);
		DEBUG_OUT("[LQRFollow] current_point: speed: " << current_point.speed.v << ", "
																<< current_point.speed.w);
		DEBUG_OUT("[LQRFollow] pre_point: x, y, theta: " << pre_point.pos.x << ", "
																<< pre_point.pos.y << ", "
																<< pre_point.pos.yaw);
		DEBUG_OUT("[LQRFollow] LQR output: v=" << out.v << ", w=" << out.w);
		DEBUG_OUT("[LQRFollow] end calculating");
	}

	DEBUG_OUT("FollowPath end");
	return ctrl_data;
}

/**
 * @brief 根据给定的参数计算并返回用于半自动跟随路径的控制数据
 *
 * 该函数根据给定的 x（横向偏差），theta（角度偏差），速度（velocity），ky（横向偏差系数），
 * ka（角度偏差系数）等参数，计算并返回用于半自动跟随路径的控制数据。
 *
 * @param x 横向偏差
 * @param theta 角度偏差
 * @param velocity 当前速度
 * @param ky 横向偏差系数
 * @param ka 角度偏差系数
 *
 * @return 返回包含控制数据的 ModifyCtrl 对象
 */
ModifyCtrl FollowPathForHalfAuto(float x, float theta, float velocity, float ky, float ka) {
	ProjectData deviation;
	// 轨迹纠偏theta角度，用来计算vx vy 分量
	Angle angle_to_tangent = Angle(0.0f);
	deviation.x_ = -x;
	deviation.theta_ = theta; // 角度归一化

	ModifyCtrl ctrl_data;
	ctrl_data.Clear();

	// #1 followPath 纠偏算法
	if (!pid_follow_half_auto.PidEnable()) {
		// followpatah纠偏算法
		float t1 = cos(deviation.theta_);
		float t2 = sin(deviation.theta_);
		float t4 = -ky * t1 * t1 * deviation.x_;
		float t5 = t2 * (ka * t1 * SIGN(velocity * t1));
		ctrl_data.angular_velocity = -velocity * t1 * (t4 + t5);

		DEBUG_OUT("halfAuto: follow: t1: " << t1 << ", t2: " << t2 << ", velocity: " << velocity
											<< ", deviation.x_: " << deviation.x_ << ", ky: " << ky
											<< ", ka: " << ka << ", t4: " << t4 << ", deviation.theta_: "
											<< deviation.theta_ << ", t5: " << t5 << ", w: " << ctrl_data.angular_velocity
											<< ", deltaVx=" << ctrl_data.delta_velocity_x << ", deltaVy=" << ctrl_data.delta_velocity_y);
	} else {
		// #2 PID纠偏算法
		bool omni_direction = IsOmniDirection(); // 是否时全向车型
		if (!omni_direction) {
			// AMR  pid纠偏算法
			float error_y = SIGN(velocity) * deviation.x_;
			if (fabs(pid_follow_half_auto.WheelBase()) > 0.0001) {
			error_y = SIGN(velocity) * atan(deviation.x_ / pid_follow_half_auto.WheelBase());
			}
			float error_a = -Angle::NormAngle2(deviation.theta_); //[-PI,PI]
																	// 将y方向偏差 -> 转化为角度偏差
			pid_follow_half_auto.SetPidFilter(velocity);
			float a_ctrl = pid_follow_half_auto.pid_a.Generate(error_a, velocity);
			float y_ctrl = pid_follow_half_auto.pid_y.Generate(error_y, velocity);
			ctrl_data.angular_velocity = a_ctrl + y_ctrl;

			DEBUG_OUT("halfAuto pid: amr: ctrl_data.fW=" << ctrl_data.angular_velocity << ", fVelX=" << ctrl_data.delta_velocity_x
														<< ", fVelY=" << ctrl_data.delta_velocity_y << ", error_y=" << error_y
														<< ", error_a=" << error_a << ", a_ctrl=" << a_ctrl
														<< ", y_ctrl=" << y_ctrl << ", deviation.x_: " << deviation.x_
														<< ", deviation.theta_: " << deviation.theta_
														<< ", velocity=" << velocity);
		} else {
			// 全向车 pid纠偏算法
			float error_y = SIGN(velocity) * deviation.x_;   // Y 方向偏差
			float error_a = -Angle::NormAngle2(deviation.theta_); //[-PI,PI]
			pid_follow_half_auto.SetPidFilter(velocity);
			float a_ctrl = pid_follow_half_auto.pid_a.Generate(error_a, velocity);
			float y_ctrl = pid_follow_half_auto.pid_y.Generate(error_y, velocity);
			ctrl_data.delta_velocity_x = sin(angle_to_tangent) * y_ctrl;
			ctrl_data.delta_velocity_y = cos(angle_to_tangent) * y_ctrl;
			ctrl_data.angular_velocity = a_ctrl;

			DEBUG_OUT("halfAuto pid: omni: ctrl_data.fW=" << ctrl_data.angular_velocity << ", fVelX=" << ctrl_data.delta_velocity_x
														<< ", fVelY=" << ctrl_data.delta_velocity_y << ", error_y=" << error_y
														<< ", error_a=" << error_a << ", angle_to_tangent(degree)="
														<< angle_to_tangent.Degree() << ", a_ctrl=" << a_ctrl
														<< ", y_ctrl=" << y_ctrl << ", velocity=" << velocity);
		}
	}

  return ctrl_data;
}